#ifndef MOTOR_H_
#define MOTOR_H_

#include "gpio.h"
#include "main.h"

//??ID
#define Motor_A 0
#define Motor_B 1
#define Motor_C 2
#define Motor_D 3

#define max_rpm 950  //????
#define max_pwm 100
#define dead_zone_rpm 3 //????


#define  PWMC(c)  __HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_1,a)
#define  PWMD(d)  __HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_2,b)
#define  PWMB(b)  __HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_4,c)
#define  PWMA(a)  __HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_1,d)
 
 
#define  CIN1_Set0  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
#define  CIN1_Set1  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
#define  CIN2_Set0  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_RESET);
#define  CIN2_Set1  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_SET);

#define  AIN1_Set0  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_RESET);
#define  AIN1_Set1  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_SET);
#define  AIN2_Set0  HAL_GPIO_WritePin(GPIOF, GPIO_PIN_11, GPIO_PIN_RESET);
#define  AIN2_Set1  HAL_GPIO_WritePin(GPIOF, GPIO_PIN_11, GPIO_PIN_SET);

#define  DIN1_Set0  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_2, GPIO_PIN_RESET);
#define  DIN1_Set1  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_2, GPIO_PIN_SET);
#define  DIN2_Set0  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_RESET);
#define  DIN2_Set1  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_SET);

#define  BIN1_Set0  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_RESET);
#define  BIN1_Set1  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_SET);
#define  BIN2_Set0  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_RESET);
#define  BIN2_Set1  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_SET);


/**
 * @brief  VelCtrl type structure definition
 * @note
 */
typedef struct
{
    float actual_speed;

    float target_speed;

    float err_now;

    float err_last;

    float err_sum;

    float acc;

    float dec;

    float kp;

    float ki;

    float kd;

    float p_out;

    float i_out;

    float d_out;

    float output;

    float maxOutput;

} Motor_Speed_Ctrl_t;

// ??????????
typedef struct {
    GPIO_TypeDef* in1_port;
    uint16_t in1_pin;
    GPIO_TypeDef* in2_port;
    uint16_t in2_pin;
  //  void (*set_pwm)(int32_t pwm); // PWM??????
} MotorPinConfig;

// ?????????
typedef struct {
    GPIO_PinState in1_forward;
    GPIO_PinState in2_forward;
    GPIO_PinState in1_reverse;
    GPIO_PinState in2_reverse;
} MotorDirection;

// ???????(?????)
typedef struct {
    MotorPinConfig pin_config;
    MotorDirection direction;
} MotorHashEntry;


extern Motor_Speed_Ctrl_t Motor_Speed_Ctrl[4];
extern float Motor_PWM[4];

void Motor_Enable(void);
void Motor_Disable(void);

float Get_Wheel_Speed(int wheel_index);
void Update_All_Speed(void);
void Motor_Calculate_Speed(int wheel_index);

void MotorInit(int Motor_ID);
void MotorCtrl(void);
float MaxMinLimit(float val, float limit);
void MotorPidCtrl(Motor_Speed_Ctrl_t *MotorPid,int Motor_ID);
float RPM_To_PWM(float rpm);

void SetSpeed(int Motor_ID, float rpm);
void Set_PWM_ALL(void);

void Motor_Control_Hash(int Motor_ID);
void Motor_Control_Optimized(float motor_pwm[4]);

#endif
